/***************************************************************************
 * @version   --> v1.0
 * @date      --> 2021-04-18
 * @brief     --> 电源管理模块
 *            1.通过串口接收上位机发送的指令，主要控制磁矩板电源、热控板电源、
 *            负载版电源、飞轮板电源的通断，并把当前各块板的电源状态和电量返回
 *              通过串口发送给上位机
 *            2.通过Arduino串口调试时，请输入(1, 1, 1, 1)的数字，顺序打开电源开关
 *@update     --> 2021-05-12
 **************************************************************************/

/*Includes --------------------------------------------------------------*/
#include <ros.h>
#include <std_msgs/UINT16.h>
#include <learning_test/EPS.h>
#include <string.h>      // 使用内存拷贝

/* Private variables ---------------------------------------------------*/
// 引脚定义
int SW0 = A7;     // 磁矩板电源开关引脚，SWITCH_0
int SW1 = A8;     // 热控板电源开关引脚，SWITCH_1
int SW2 = A9;     // 负载板电源开关引脚，SWITCH_2
int SW3 = A10;    // 飞轮板电源开关引脚，SWITCH_3

// 开关状态记录
volatile bool P0_on = false;
volatile bool P1_on = false;
volatile bool P2_on = false;
volatile bool P3_on = false;

ros::NodeHandle nh;
learning_test::EPS state_msg;
std_msgs::UInt16 ter_power;
std_msgs::UInt16 fw_power;
std_msgs::UInt16 mag_power;
std_msgs::UInt16 pay_power;

void subscriberCallback0(const std_msgs::UInt16& ter_power){
  // 读取热控板电源状态
  if(ter_power.data == 1){
    digitalWrite(SW1, HIGH);
    P1_on = true;
  }
  else{
    digitalWrite(SW1, LOW);
    P1_on = false;
  }
}

void subscriberCallback1(const std_msgs::UInt16& fw_power){
  // 读取飞轮板电源状态
  if(fw_power.data == 1){
    digitalWrite(SW3, HIGH);
    P3_on = true;
  }
  else{
    digitalWrite(SW3, LOW);
    P3_on = false;
  }
}

void subscriberCallback2(const std_msgs::UInt16& mag_power){
  // 读取磁矩板电源状态
  if(mag_power.data == 1){
    digitalWrite(SW0, HIGH);
    P0_on = true;
  }
  else{
    digitalWrite(SW0, LOW);
    P0_on = false;
  }
}

void subscriberCallback3(const std_msgs::UInt16& pay_power){
  // 读取载荷板电源状态
  if(pay_power.data == 1){
    digitalWrite(SW2, HIGH);
    P2_on = true;
  }
  else{
    digitalWrite(SW2, LOW);
    P2_on = false;
  }
}
 
ros::Publisher EPS_publisher("power_state", &state_msg);
ros::Subscriber<std_msgs::UInt16> ter_subscriber("ter_power", &subscriberCallback0);
ros::Subscriber<std_msgs::UInt16> fw_subscriber("fw_power", &subscriberCallback1);
ros::Subscriber<std_msgs::UInt16> mag_subscriber("mag_power", &subscriberCallback2);
ros::Subscriber<std_msgs::UInt16> pay_subscriber("pay_power", &subscriberCallback3);

void setup() {
  Serial.begin(57600);     // 使用串口通信
  delay(20);
  // 开关引脚初始化
  pinMode(SW0, OUTPUT);
  pinMode(SW1, OUTPUT);
  pinMode(SW2, OUTPUT);
  pinMode(SW3, OUTPUT);
  nh.initNode();
  nh.advertise(EPS_publisher);
  nh.subscribe(ter_subscriber);
  nh.subscribe(fw_subscriber);
  nh.subscribe(mag_subscriber);
  nh.subscribe(pay_subscriber);
}

void loop() {
  Serial.println("此时各模块工作状态：");
  Serial.print("磁矩板电源开关：");
  Serial.println(P0_on);
  Serial.print("热控板电源开关：");
  Serial.println(P1_on);
  Serial.print("负载板电源开关：");
  Serial.println(P2_on);
  Serial.print("飞轮板电源开关：");
  Serial.println(P3_on);
  
  state_msg.ter_sw = P1_on;
  state_msg.fw_sw = P3_on;
  state_msg.mag_sw = P0_on;
  state_msg.pay_sw = P2_on;
  EPS_publisher.publish(&state_msg);
  nh.spinOnce();
  delay(20);
}
